#include "fxp.h"
#include <limits.h>

// Fixed point arithmetic operations

// 16 bit add
int16_t fxp_add(int16_t var1,int16_t var2) {
	return (var1>32767-var2)?32767:((var1<-32768-var2)?-32768:var1+var2);
}
// 16 bit sub
int16_t fxp_sub(int16_t var1,int16_t var2) {
	return (var1>32767+var2)?32767:((var1<-32768+var2)?-32768:var1-var2);
}
// 16 bit mult
int16_t fxp_mult(int16_t var1,int16_t var2) {
	if(var1==-32768 && var2==-32768) return 32767;
	return (int16_t)((int32_t)(var1*var2)>>15);
}
// 16 bit mult with round
int16_t fxp_mult_r(int16_t var1,int16_t var2) {
	if(var1==-32768 && var2==-32768) return 32767;
	return (int16_t)((int64_t)(var1*var2+16384)>>15);
}
// 16 bit abs
int16_t fxp_abs(int16_t var1) {
	if(var1==-32768) return 32767;
	return var1<0?-var1:var1;
}
// 32 bit add
int32_t fxp_L_add(int32_t L_var1,int32_t L_var2) {
	return (L_var1>2147483647LL-L_var2)?INT_MAX:((L_var1<INT_MIN-L_var2)?-2147483648LL:L_var1+L_var2);
}
// 32 bit sub
int16_t fxp_L_sub(int16_t L_var1,int16_t L_var2) {
	return (L_var1>2147483647LL+L_var2)?2147483647LL:((L_var1<-2147483648LL+L_var2)?-2147483648LL:L_var1-L_var2);
}
// 32 bit mult
int16_t fxp_L_mult(int16_t L_var1,int16_t L_var2) {
	return (L_var1*L_var2)<<1;
}
// 32 bit norm(returns -1 if it fails)
int16_t fxp_L_norm(int32_t L_var1) {
	if(!L_var1) return -1;
	register int32_t limit=L_var1<0?-1073741824:1073741824;
	int8_t norm=0;
	while(1) {
		if((L_var1>0 && L_var1>=limit)||(L_var1<=0 && L_var1<=limit))
			return norm;
		norm++;
		L_var1<<=1;
		if(norm>15) break;
	}
	return -1;
}
// 16 bit div
int16_t fxp_div(int16_t var1,int16_t var2) {
	int32_t L_num=var1;
	int32_t L_denum=var2;
	int16_t div=0;
	int8_t k;
	for(k=0;k<15;k++) {
		div<<=1;
		L_num<<=1;
		if(L_num>=L_denum) {
			L_num=fxp_L_sub(L_num,L_denum);
			div=fxp_add(div,1);
		}
	}
	return div;
}


